#include "visionlocation.h"
#include <QThread>
#include <qelapsedtimer.h>

#define PI 3.1415926535898

VisionLocation::VisionLocation(SCVision *vision, SCCamera *camera, VisionLocationConfig *config)
    : vision(vision), m_camera(camera), m_config(config)
{
    connect(config, &VisionLocationConfig::lightBrightnessChanged, this,
            &VisionLocation::onVisionLocationConfigLightBrightnessChanged, Qt::DirectConnection);
    connect(config, &VisionLocationConfig::secondLightBrightnessChanged, this,
            &VisionLocation::onVisionLocationConfigSecondLightBrightnessChanged, Qt::DirectConnection);
}

void VisionLocation::setMapping(Pixel2Mech *mapping)
{
    this->mapping = mapping;
}

bool VisionLocation::performPR(PrOffset &offset, int roiRowIndex, int roiColIndex)
{
    offset.ReSet();
    PRResultStruct prResult;
    if (performPR(prResult, roiRowIndex, roiColIndex))
    {
        return result2Offset(prResult, offset);
    }
    else
    {
        return false;
    }
}

void VisionLocation::startContinuallyPerformPr()
{
    if (isContinuallyPerformPr)
    {
        return;
    }
    isContinuallyPerformPr = true;

    if(m_config->continuoullyPrROICount() < 1)
    {
        m_config->setContinuoullyPrROICount(1);
    }

    int continuoullyPrROICount = m_config->continuoullyPrROICount();
    QList<QString> continuallyPrLogFileNames;
    for (int i = 0; i < continuoullyPrROICount; ++i)
    {
        continuallyPrLogFileNames.append(QString("./continuallyPrData/%1_ROI%2_%3.csv")
                                         .arg(m_config->locationName())
                                         .arg(i)
                                         .arg(QDateTime::currentDateTime().toString("yyyy_MM_dd_hh_mm_ss_zzz")));
    }

    PrOffset prOffset;
    PRResultStruct prResult;
    while (isContinuallyPerformPr)
    {
        QImage img = getImage();
        for (int i = 0; i < continuoullyPrROICount; ++i)
        {
            prOffset.ReSet();
            try
            {
                performPR(img, prResult, 0, i);
            }
            catch (SilicoolException &se)
            {
                isContinuallyPerformPr = false;
                throw se;
            }
            result2Offset(prResult, prOffset);

            QFile logFile(continuallyPrLogFileNames[i]);
            QDir dir("./continuallyPrData");
            if (!dir.exists())
            {
                dir.mkpath(dir.absolutePath());
            }
            bool fileExist = logFile.exists();

            logFile.open(QIODevice::Append);
            if (!fileExist)
            {
                QString title = "timeStamp,offsetX_mm,offsetY_mm,offsetTheta_degree,offsetX_pixel,offsetY_pixel\r\n";
                logFile.write(title.toUtf8());
            }
            QString line = QString("%1,%2,%3,%4,%5,%6\r\n").arg(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss.zzz"))
                                                                .arg(prOffset.X).arg(prOffset.Y).arg(prOffset.Theta)
                                                                .arg(prResult.x).arg(prResult.y);
            logFile.write(line.toUtf8());
            logFile.flush();
            logFile.close();

            if(m_config->continuoullyPrDelay() > 0)
            {
                QThread::msleep(m_config->continuoullyPrDelay());
            }
        }
    }
}

void VisionLocation::stopContinuallyPerformPr()
{
    isContinuallyPerformPr = false;
}

bool VisionLocation::performPR(PRResultStruct &prResult, int roiRowIndex, int roiColIndex)
{
    QImage image = getImage();
    return performPR(image, prResult, roiRowIndex, roiColIndex);
}

bool VisionLocation::performPR(QImage &image, PRResultStruct &prResult, int roiRowIndex, int roiColIndex)
{
    PRResultImageInfo resultImageInfo;
    SCVision::PrType prType = SCVision::PrTypeEnumInfo().nameToEnum(m_config->prType());
    bool res = vision->performPr(image, prFileName(), prType, m_config->objectScore(), resultImageInfo, prResult,
                                 roiRowIndex, roiColIndex);
    prResult.theta *= -1;    // Vision计算角度时顺时针为正，应用层使用时定义逆时针为正

    if (res)
    {
        saveShowResultImage(image, resultImageInfo);
        qCInfo(visionCate()) << m_config->locationName() << "pr result:" << prResult.toString();
    }
    else
    {
        tryShowImage(image);
        qCCritical(visionCate()) << m_config->locationName() << "perform pr failed!";
    }
    return res;
}

void VisionLocation::openLight()
{
    m_camera->setTwoChanndelBrightness(m_config->lightBrightness(), m_config->secondLightBrightness(), true);
}

void VisionLocation::closeLight()
{
    m_camera->setTwoChanndelBrightness(0, 0, false);
}

QImage VisionLocation::getImage()
{
    return ErrorHandler::tryToHandleGeneralError<QImage>([this] {
        openLight();
        if (m_config->waitImageDelay() > 0)
        {
            QThread::msleep(m_config->waitImageDelay());
        }
        QImage image = m_camera->getImage(false, m_config->saveImageAfterGrab());
        if (m_camera->config()->closeLightAfterPr())
        {
            closeLight();
        }
        return image;
    });
}

bool VisionLocation::performPR(QImage image, PrOffset &offset, bool showImage, int roiRowIndex, int roiColIndex)
{
    offset.ReSet();
    PRResultStruct prResult;
    PRResultImageInfo resultImageInfo;
    SCVision::PrType prType = SCVision::PrTypeEnumInfo().nameToEnum(m_config->prType());
    bool res = vision->performPr(image, prFileName(), prType, m_config->objectScore(), resultImageInfo, prResult,
                                 roiRowIndex, roiColIndex);
    prResult.theta *= -1;    // Vision计算角度时顺时针为正，应用层使用时定义逆时针为正

    if (res)
    {
        saveShowResultImage(image, resultImageInfo, showImage);
        qCInfo(visionCate()) << m_config->locationName() << "pr result:" << prResult.toString();
        return result2Offset(prResult, offset);
    }
    else
    {
        if (showImage)
        {
            tryShowImage(image);
        }
        qCCritical(visionCate()) << m_config->locationName() << "perform pr failed!";
        return false;
    }
}

bool VisionLocation::result2Offset(PRResultStruct &prResult, PrOffset &prOffset)
{
    prOffset.ReSet();
    QPointF mech;
    QPointF mech_o;
    if (mapping->CalcMechDistance(QPointF(prResult.x, prResult.y), mech)
        && mapping->CalcMechDistance(QPointF(prResult.ori_x, prResult.ori_y), mech_o))
    {
        prOffset.X = mech.x();
        prOffset.Y = mech.y();
        prOffset.O_X = mech_o.x();
        prOffset.O_Y = mech_o.y();

        if (prOffset.O_X < m_config->minXOffset() || prOffset.O_X > m_config->maxXOffset()
            || prOffset.O_Y < m_config->minYOffset() || prOffset.O_Y > m_config->maxYOffset())
        {
            QString errMsg
                = QObject::tr("Pr offset exceeded limit! Offset: %1, OffsetX limit: [%2, %3], OffsetY limit: [%4, %5]")
                      .arg(prOffset.toString())
                      .arg(m_config->minXOffset())
                      .arg(m_config->maxXOffset())
                      .arg(m_config->minYOffset())
                      .arg(m_config->maxYOffset());
            qCCritical(visionCate()) << m_config->locationName() << errMsg;
            return false;
        }

        if (abs(prResult.theta) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta;
        }
        else if (abs(prResult.theta - 90) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta - 90;
        }
        else if (abs(prResult.theta + 90) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta + 90;
        }
        else if (abs(prResult.theta - 180) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta - 180;
        }
        else if (abs(prResult.theta + 180) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta + 180;
        }
        else if (abs(prResult.theta - 270) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta - 270;
        }
        else if (abs(prResult.theta + 270) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta + 270;
        }
        else if (abs(prResult.theta - 360) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta - 360;
        }
        else if (abs(prResult.theta + 360) < m_config->maximunAngle())
        {
            prOffset.Theta = prResult.theta + 360;
        }
        else
        {
            QString errMsg = QObject::tr("Pr offset theta exceeded limit! Theta: %1, Theta limit: %2")
                                 .arg(prResult.theta)
                                 .arg(m_config->maximunAngle());
            qCCritical(visionCate()) << m_config->locationName() << errMsg;
            return false;
        }
        qCInfo(visionCate()) << m_config->locationName() << "pr offset:" << prOffset.toString();
        return true;
    }
    else
    {
        throw SilicolAbort(tr("%1 Pixel to mech failed!").arg(m_config->locationName()), EX_LOCATION);
    }
}

SCCamera *VisionLocation::camera() const
{
    return m_camera;
}

void VisionLocation::onVisionLocationConfigLightBrightnessChanged(int lightBrightnessChanged)
{
    try
    {
        m_camera->setLightSourceBrightness(lightBrightnessChanged);
    }
    catch (SilicoolException &se)
    {
        qCCritical(visionCate()) << se.what();
    }
}

void VisionLocation::onVisionLocationConfigSecondLightBrightnessChanged(int lightBrightnessChanged)
{
    try
    {
        m_camera->setSecondLightSourceBrightness(lightBrightnessChanged);
    }
    catch (SilicoolException &se)
    {
        qCCritical(visionCate()) << se.what();
    }
}

void VisionLocation::tryShowImage(QImage &image)
{
    if (!m_camera->isRunning())
    {
        m_camera->showImage(image);
    }
}

QString VisionLocation::prFileName() const
{
    return QFileInfo(m_config->prFileName()).absoluteFilePath();
}

void VisionLocation::saveShowResultImage(QImage &image, PRResultImageInfo &prResultImageInfo, bool showImage)
{
    QtConcurrent::run([image, prResultImageInfo, showImage, this] {
        PRResultImageInfo tmpPrResultImageInfo = prResultImageInfo;
        QImage tmpImage = image;
        QString imageName = QString("%1_%2_resultImage.jpg")
                                .arg(m_camera->cameraName())
                                .arg(QDateTime::currentDateTime().toString("hh_mm_ss_zzz"));
        vision->drawResultImage(tmpImage, tmpPrResultImageInfo);
        if (showImage)
        {
            tryShowImage(tmpImage);
        }
        if (m_config->saveResultImage())
        {
            m_camera->saveImage(tmpImage, "", imageName);
        }
    });
}
